#include "ultrasonic.h"

void *ultrasonic_thread(void *argv) {
    int32_t i, ret;
    uint8_t cmd, ack[ULTRASONIC_ACK_MAX];
    Ultrasonic::ULTRASONICPARAM *param = (Ultrasonic::ULTRASONICPARAM *)argv;
    // 循环读取超声波数据
    while (*param->signal) {
        // 重置数据
        ret = -1, cmd = 0x55;
        memset(ack, 0, sizeof(ack));
        // 测距信号
        write(*param->fd, &cmd, sizeof(cmd));
        // 等待数据
        sleep(1);
        // 获取数据
        if ((ret = read(*param->fd, ack, sizeof(ack))) != 2) {
            printf("abnormal ultrasonic ranging! ret:%d\n", ret);
            continue;
        }
        // 保存数据
        for (i = 0; i < ULTRASONIC_ACK_MAX; i++)
            (*param->ack)[i] = ack[i];
        // 保存距离
        *param->distance = ack[0] << 8 | ack[1];
    }
    // 退出线程
    pthread_exit(param->tid);
    return NULL;
}

Ultrasonic::Ultrasonic(const string path, const uint32_t baudrate) {
    struct termios termios;
    // 初始化数据
    this->fd = -1, this->tid = -1, this->signal = 0, this->distance = 0;
    vector< uint8_t >().swap(this->ack);
    memset(&this->param, 0, sizeof(this->param));
    // 打开超声波传感器
    if ((this->fd = open(path.c_str(), O_RDWR)) == -1)
        throw runtime_error("open '" + path + "' error!\n");
    // 清空数据
    memset(&termios, 0, sizeof(termios));
    // 设置数据
    termios.c_cflag |= (CLOCAL | CREAD);    // O_RDWR
    termios.c_cflag &= ~CSIZE;              // 清空数据位
    termios.c_cflag &= ~ULTRASONIC_CRTSCTS; // 无硬件控制
    termios.c_cflag |= CS8;                 // 数据位：8
    termios.c_cflag &= ~CSTOPB;             // 停止位：1
    termios.c_cflag &= ~PARENB;             // 无校验模式
    // 设置波特率
    switch (baudrate) {
    case 9600:
        cfsetospeed(&termios, B9600);
        cfsetispeed(&termios, B9600);
        break;
    case 115200:
        cfsetospeed(&termios, B115200);
        cfsetispeed(&termios, B115200);
        break;
    case 19200:
        cfsetospeed(&termios, B19200);
        cfsetispeed(&termios, B19200);
        break;
    }
    // 刷新输出队列
    tcflush(this->fd, TCIFLUSH);
    // 改变配置
    tcsetattr(this->fd, TCSANOW, &termios);
    // 设置数据最大长度
    this->ack.resize(ULTRASONIC_ACK_MAX);
    //  创建线程
    this->signal = 1;
    this->param.fd = &this->fd;
    this->param.tid = &this->tid;
    this->param.signal = &this->signal;
    this->param.distance = &this->distance;
    this->param.ack = &this->ack;
    if (pthread_create(&this->tid, NULL, ultrasonic_thread, &this->param)) {
        close(this->fd);
        throw runtime_error("ultrasonic pthread_create error!\n");
    }
}

Ultrasonic::~Ultrasonic() {
    // 关闭线程
    if (this->signal != 0)
        this->signal = 0;
    // 关闭文件
    if (this->fd != -1)
        close(this->fd);
}

uint32_t &Ultrasonic::get_distance() {
    return this->distance;
}

vector< uint8_t > &Ultrasonic::get_ack() {
    return this->ack;
}